#include "gimbal_task.h"
#include "CAN_receive.h"
#include "INS_task.h"
#include "NUC_communicate.h"
#include "OLED.h"
#include "arm_math.h"
#include "bsp_led.h"
#include "cmsis_os.h"
#include "detect_task.h"
#include "main.h"
#include "pid.h"
#include "remote_control.h"
#include "robot_config.h"
#include "robot_total_mode_task.h"
#include "user_lib.h"
#include "struct_typedef.h"
#include "referee.h"
#include "shoot_task.h"
#include "debug_config.h"

#ifndef SENTRY_CHASSIS
#define GIMBAL_TASK_INIT_TIME 2000
#define GIMBAL_TASK_CTRL_TIME 2

#define SAFE_AUTO_SHIFT_TIME 1500 // 自动模式时，当装甲板丢失时，使用手动控制，但不清空操作手鼠标偏移量。当重新从手动进入自动模式模式时，操作手可能因为来不及反应,导致偏移量破坏。因此设计安全时间，当重新进入时，有一段时间无视操作手新增偏移量。这段时间为SAFE_AUTO_SHIFT_TIME 单位ms

#ifdef INFANTRY
#define PITCH_TEST_RAD_MIN (-0.20f)
#define PITCH_TEST_RAD_MAX (0.26f)
#endif

#ifdef HERO
#define PITCH_TEST_RAD_MIN (-0.40f)
#define PITCH_TEST_RAD_MAX (0.68f)
#endif

#ifdef SENTRY
#define PITCH_TEST_RAD_MIN (-0.4f)
#define PITCH_TEST_RAD_MAX (0.48f)
#endif

/**********************PID控制参数********************/
#ifdef INFANTRY
// 输入角度 rad ，输出角速度rad/s 的PID系数
#define PITCH_AGL_KP 20.0f
#define PITCH_AGL_KI 0.0f
#define PITCH_AGL_KD 0.0f
#define PITCH_AGL_SPD_MAX_OUT (100.0f)
#define PITCH_AGL_SPD_MAX_IOUT (0.7f)

// 输入角速度 rad/s 、输出电压 int16_t 的PID系数
#define PITCH_SPD_KP 8000.0f
#define PITCH_SPD_KI 0.0f
#define PITCH_SPD_KD 0.0f
#define PITCH_VOLT_MAX_OUT 20000.0f
#define PITCH_VOLT_MAX_IOUT 5000.0f

#define YAW_AGL_KP 25.0f
#define YAW_AGL_KI 0.0f
#define YAW_AGL_KD 0.0f
#define YAW_AGL_SPD_MAX_OUT (1000.0f)
#define YAW_AGL_SPD_MAX_IOUT (0.7f)

#define YAW_SPD_KP 10000.0f
#define YAW_SPD_KI 0.0f
#define YAW_SPD_KD 0.0f
#define YAW_VOLT_MAX_OUT 14000.0f
#define YAW_VOLT_MAX_IOUT 5000.0f
#endif

#ifdef HERO
#define PITCH_AGL_KP 20.0f
#define PITCH_AGL_KI 0.0f
#define PITCH_AGL_KD 0.0f
#define PITCH_AGL_SPD_MAX_OUT (100.0f)
#define PITCH_AGL_SPD_MAX_IOUT (0.7f)

#define PITCH_SPD_KP 3000.0f
#define PITCH_SPD_KI 0.0f
#define PITCH_SPD_KD 0.0f
#define PITCH_VOLT_MAX_OUT 20000.0f
#define PITCH_VOLT_MAX_IOUT 5000.0f

#define YAW_AGL_KP 25.0f
#define YAW_AGL_KI 0.0f
#define YAW_AGL_KD 0.0f
#define YAW_AGL_SPD_MAX_OUT (1000.0f)
#define YAW_AGL_SPD_MAX_IOUT (0.7f)

#define YAW_SPD_KP 4500.0f
#define YAW_SPD_KI 0.0f
#define YAW_SPD_KD 0.0f
#define YAW_VOLT_MAX_OUT 11000.0f
#define YAW_VOLT_MAX_IOUT 5000.0f
#endif

#ifdef SENTRY
#define PITCH_AGL_KP 15.0f
#define PITCH_AGL_KI 0.0f
#define PITCH_AGL_KD 0.0f
#define PITCH_AGL_SPD_MAX_OUT (30.0f)
#define PITCH_AGL_SPD_MAX_IOUT (0.7f)

#define PITCH_SPD_KP 1500.0f
#define PITCH_SPD_KI 0.5f
#define PITCH_SPD_KD 3.0f
#define PITCH_VOLT_MAX_OUT 30000.0f
#define PITCH_VOLT_MAX_IOUT 40.0f

#define YAW_AGL_KP 25.0f
#define YAW_AGL_KI 0.0f
#define YAW_AGL_KD 0.0f
#define YAW_AGL_SPD_MAX_OUT (80.0f)
#define YAW_AGL_SPD_MAX_IOUT (0.7f)

#define YAW_SPD_KP 60000.0f
#define YAW_SPD_KI 0000.0f
#define YAW_SPD_KD 000.0f
#define YAW_VOLT_MAX_OUT 30000.0f
#define YAW_VOLT_MAX_IOUT 5000.0f
#endif

#define PID_DEATH_AREA 0.005 //防止I不断累积


#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }

#define range_limit_inside(toBeLimited, range) \
    {                                          \
        if ((toBeLimited) > (range))           \
        {                                      \
            (toBeLimited) = (range);           \
        }                                      \
        else if ((toBeLimited) < -(range))     \
        {                                      \
            (toBeLimited) = -(range);          \
        }                                      \
    }

struct gimbalMotorCtrl_t
{
    // rads
    const fp32 *anglePoint;   // 从姿态解算任务得到的角度位置
    const fp32 *gyroPointer;  // 陀螺仪角速度指针
    fp32 nowAbsoluteAngle;    // 当前姿态角
    fp32 radSpeed;            // 角速度 rad/s
    fp32 wantedAbsoluteAngle; // 目标姿态角

    // PIDs
    pid_type_def spdPid; // 用于控制速度的PID环。输入角速度 rad/s, 输出GM6020电压 int16
    pid_type_def aglPid; // 用于控制角度的PID环。输入角度 rad,输出角速度rad/s

    // 控制量
    int16_t giveVolt; // GM6020发送电压
    uint8_t zeroVoltMark;
};

static struct gimbalMotorCtrl_t gimbalPitchCtrl, gimbalYawCtrl;
static struct gimbalMotorCtrl_t *gimbalCtrl[2] = {&gimbalPitchCtrl, &gimbalYawCtrl};

static fp32 yawSpdPID[3] = {YAW_SPD_KP, YAW_SPD_KI, YAW_SPD_KD};
static fp32 pitchSpdPID[3] = {PITCH_SPD_KP, PITCH_SPD_KI, PITCH_SPD_KD};
static fp32 yawAglPID[3] = {YAW_AGL_KP, YAW_AGL_KI, YAW_AGL_KD};
static fp32 pitchAglPID[3] = {PITCH_AGL_KP, PITCH_AGL_KI, PITCH_AGL_KD};

static const uint8_t *robotMode;
// 控制指针
static const RC_ctrl_t *rc_p;  // 遥控器位置指针，需要初始化
static const fromNUC_t *nuc_p; // NUC数据位置。未来制造全自动机器人时需要
static const remote_control_t* remoteControl_p; //图传链路控制
#ifndef SENTRY 
const int16_t *triggerCurrent_p;
#endif
#ifdef GIMBAL_WITHOUT_IMU
const int16_t *pitchUpVoltage_p;
const int16_t *yawUpVoltage_p;
#endif

typedef struct
{
    fp32 LastP; // 上次估算协方差 初始化值为0.02
    fp32 Now_P; // 当前估算协方差 初始化值为0
    fp32 out;   // 卡尔曼滤波器输出 初始化值为0
    fp32 Kg;    // 卡尔曼增益 初始化值为0
    fp32 Q;     // 过程噪声协方差 初始化值为0.00001
    fp32 R;     // 观测噪声协方差 初始化值为0.00003
} KFP_t;        // Kalman Filter parameter

KFP_t KfpPitchWantAgl = {0.02, 0, 0, 0.9, 0.0001, 0.0003};
KFP_t KfpYawWantAgl = {0.02, 0, 0, 0.9, 0.0001, 0.0003};

fp32 kalmanFilter(KFP_t *kfp, fp32 input)
{
    // 预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
    kfp->Now_P = kfp->LastP + kfp->Q;
    // 卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    //  kfp->Kg = kfp->Now_P / (kfp->Now_P + kfp->R);

    // 更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    kfp->out = kfp->out + kfp->Kg * (input - kfp->out); // 因为这一次的预测值就是上一次的输出值
    // 更新协方差方程: 本次的系统协方差付给 kfp->LastP 威下一次运算准备。
    kfp->LastP = (1 - kfp->Kg) * kfp->Now_P;
    return kfp->out;
}

static void init_gimbal_ctrls(void)
{
    uint8_t i;
    // 初始化绝对角度位置
    gimbalYawCtrl.anglePoint = get_INS_angle_point();           // 第一个角度
    #ifdef INFANTRY
    gimbalPitchCtrl.anglePoint = (&(get_INS_angle_point()[2])); // 第三个角度
    #else
    gimbalPitchCtrl.anglePoint = (&(get_INS_angle_point()[1]));
    #endif
    gimbalYawCtrl.gyroPointer = &(get_gyro_data_point()[2]);
    #ifdef INFANTRY
    gimbalPitchCtrl.gyroPointer = &(get_gyro_data_point()[0]);
    #else
    gimbalPitchCtrl.gyroPointer = &(get_gyro_data_point()[1]);
    #endif

    // 初始化PID参数
    // 两个双环PID不共用参数
    PID_init(&(gimbalPitchCtrl.spdPid), PID_POSITION, pitchSpdPID, PITCH_VOLT_MAX_OUT, PITCH_VOLT_MAX_IOUT);
    PID_init(&(gimbalYawCtrl.spdPid), PID_POSITION, yawSpdPID, YAW_VOLT_MAX_OUT, YAW_VOLT_MAX_IOUT);

    PID_init(&(gimbalPitchCtrl.aglPid), PID_POSITION, pitchAglPID, PITCH_AGL_SPD_MAX_OUT, PITCH_AGL_SPD_MAX_IOUT);
    PID_init(&(gimbalYawCtrl.aglPid), PID_POSITION, yawAglPID, YAW_AGL_SPD_MAX_OUT, YAW_AGL_SPD_MAX_IOUT);


    // 初始化当前角度和目标角度、pid参数、里程碑栈法数据
    for (i = 0; i < 2; i++)
    {
        (gimbalCtrl[i])->zeroVoltMark = 0;
        (gimbalCtrl[i])->nowAbsoluteAngle = *((gimbalCtrl[i])->anglePoint);
        gimbalCtrl[i]->wantedAbsoluteAngle = (gimbalCtrl[i])->nowAbsoluteAngle;
    }
    PID_clear(&gimbalYawCtrl.aglPid);
    PID_clear(&gimbalYawCtrl.spdPid);
    PID_clear(&gimbalPitchCtrl.aglPid);
    PID_clear(&gimbalPitchCtrl.spdPid);
    gimbalYawCtrl.radSpeed = 0;
    gimbalPitchCtrl.radSpeed = 0;
}

fp32 radFormat(fp32 rawAngle)
{
    while (rawAngle > PI)
        rawAngle -= (2 * PI);
    while (rawAngle < (-PI))
        rawAngle += (2 * PI);
    return rawAngle;
}

fp32 gimbal_PID_calc(pid_type_def *pid, fp32 ref, fp32 set)
{
    if (pid == NULL)
    {
        return 0.0f;
    }

    pid->error[2] = pid->error[1];
    pid->error[1] = pid->error[0];
    pid->set = set;
    pid->fdb = ref;
    pid->error[0] = radFormat(set - ref);
    if (pid->mode == PID_POSITION)
    {
        pid->Pout = pid->Kp * pid->error[0];
        pid->Iout += pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        range_limit_inside(pid->Iout, pid->max_iout);
        pid->out = pid->Pout + pid->Iout + pid->Dout;
        range_limit_inside(pid->out, pid->max_out);
    }
    else if (pid->mode == PID_DELTA)
    {
        pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
        pid->Iout = pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        pid->out += pid->Pout + pid->Iout + pid->Dout;
        range_limit_inside(pid->out, pid->max_out);
    }
    return pid->out;
}

void refresh_angle_states(void)
{
    // 当前姿态角和角速度
    gimbalYawCtrl.nowAbsoluteAngle = *(gimbalYawCtrl.anglePoint);
    gimbalYawCtrl.radSpeed = *(gimbalYawCtrl.gyroPointer);
    #ifdef INFANTRY
    gimbalPitchCtrl.nowAbsoluteAngle = *(gimbalPitchCtrl.anglePoint);
    gimbalPitchCtrl.radSpeed = *(gimbalPitchCtrl.gyroPointer);
    #else
    gimbalPitchCtrl.nowAbsoluteAngle =-*(gimbalPitchCtrl.anglePoint);
    gimbalPitchCtrl.radSpeed =-*(gimbalPitchCtrl.gyroPointer);
    #endif
}
#ifdef STM_STUDIO_PITCH_CHANNEL
int16_t stm_pitch_channel;
#endif


void get_control_angles(void)
{
    static fp32 Manual_selfTargetOffsetPitch = 0, Manual_selfTargetOffsetYaw = 0;
    static int16_t yaw_channel = 0, pitch_channel = 0;
    rc_deadband_limit(rc_p->rc.ch[RC_GIMBAL_YAW], yaw_channel, GIMBAL_RC_DEADLINE);
    rc_deadband_limit(rc_p->rc.ch[RC_GIMBAL_PITCH], pitch_channel, GIMBAL_RC_DEADLINE);
    #ifdef STM_STUDIO_PITCH_CHANNEL
    stm_pitch_channel=pitch_channel;
    #endif
    static uint8_t lastMode = ROBOT_STATE_POWERLESS; // 初始化为无力
    if (lastMode != (*robotMode))
    {
        gimbalYawCtrl.wantedAbsoluteAngle = gimbalYawCtrl.nowAbsoluteAngle;
    }
    lastMode = *robotMode;
    gimbalYawCtrl.zeroVoltMark = 0;
    gimbalPitchCtrl.zeroVoltMark = 0;
    if (*robotMode == ROBOT_STATE_POWERLESS)
    {
        gimbalYawCtrl.zeroVoltMark = 1;
        gimbalPitchCtrl.zeroVoltMark = 1;
        Manual_selfTargetOffsetPitch = 0;
        Manual_selfTargetOffsetYaw = 0;
        gimbalPitchCtrl.wantedAbsoluteAngle = 0;
    }
    else if (*robotMode == ROBOT_STATE_AUTO)
    {
        if (gimbal_NUC_is_available())
        {
            #ifdef USE_USART
            gimbalYawCtrl.wantedAbsoluteAngle = gimbalYawCtrl.nowAbsoluteAngle + nuc_p->yaw * PI / (fp32)INT16_MAX;
            gimbalPitchCtrl.wantedAbsoluteAngle = ((fp32)nuc_p->pitch) * 0.3 / (fp32)INT8_MAX;
            #endif
            #ifdef USE_CAN
            gimbalYawCtrl.wantedAbsoluteAngle = gimbalYawCtrl.nowAbsoluteAngle + (fp32)nuc_p->yaw / (fp32)(INT16_MAX>>2);
            gimbalPitchCtrl.wantedAbsoluteAngle =gimbalPitchCtrl.nowAbsoluteAngle+ ((fp32)nuc_p->pitch) * 0.3f/ (fp32)INT8_MAX;
            #endif
            gimbal_clear_read_mark();
        }
    }
    else
    {
        #ifdef SENTRY
        if(switch_is_down(rc_p->rc.ch[RC_CHOOSE_Y_OR_W])){
        #endif
        if (!robot_is_auxiliaryTarget()) // 当自瞄取消掉的时候，偏移清零
        {
            Manual_selfTargetOffsetPitch = 0;
            Manual_selfTargetOffsetYaw = 0;
            gimbalYawCtrl.wantedAbsoluteAngle += yaw_channel * YAW_RC_SEN + remoteControl_p->mouse_x * PITCH_MOUSE_SEN;
            gimbalPitchCtrl.wantedAbsoluteAngle += pitch_channel * PITCH_RC_SEN + remoteControl_p->mouse_y * PITCH_MOUSE_SEN;
        }
        else
        {
            static uint32_t lastManualTime = 0;
            if ((HAL_GetTick() - lastManualTime) > SAFE_AUTO_SHIFT_TIME) // 只有过了缓冲时间之后才认定偏移量
            {
                // pitch和yaw偏移,并增加到角度上。
                Manual_selfTargetOffsetPitch += pitch_channel * PITCH_RC_SEN + remoteControl_p->mouse_y * PITCH_MOUSE_SEN;
                Manual_selfTargetOffsetYaw += yaw_channel * YAW_RC_SEN + remoteControl_p->mouse_x * YAW_MOUSE_SEN;
            }

            // 给偏移量限幅
            range_limit_inside(Manual_selfTargetOffsetPitch, PI / 3);
            range_limit_inside(Manual_selfTargetOffsetYaw, PI / 3);
            if (found_armor()) // 找到了才让云台跟随NUC数据
            {
                if (gimbal_NUC_is_available())
                {
                    gimbalYawCtrl.wantedAbsoluteAngle += nuc_p->yaw/(fp32)(INT16_MAX>>2) + Manual_selfTargetOffsetYaw;
                    gimbalPitchCtrl.wantedAbsoluteAngle += nuc_p->pitch* 0.3f/(fp32)INT8_MAX + Manual_selfTargetOffsetPitch;
                    gimbal_clear_read_mark();
                }
            }
            else
            {
                lastManualTime = HAL_GetTick(); // 更新auto模式下手动的时间
                // 直接复制手动控制的代码
                gimbalYawCtrl.wantedAbsoluteAngle += yaw_channel * YAW_RC_SEN + remoteControl_p->mouse_x * PITCH_MOUSE_SEN;
                gimbalPitchCtrl.wantedAbsoluteAngle += pitch_channel * PITCH_RC_SEN + remoteControl_p->mouse_y * PITCH_MOUSE_SEN;
            }
        }
        gimbalYawCtrl.wantedAbsoluteAngle = kalmanFilter(&KfpYawWantAgl, gimbalYawCtrl.wantedAbsoluteAngle);
        gimbalPitchCtrl.wantedAbsoluteAngle = kalmanFilter(&KfpPitchWantAgl, gimbalPitchCtrl.wantedAbsoluteAngle);
        #ifdef SENTRY
        }
        #endif
    }
}

void limit_angles()
{
    struct gimbalMotorCtrl_t *c;
    int i;
    for (i = 0; i < 2; i++)
    {
        c = gimbalCtrl[i];
        if (radFormat(c->wantedAbsoluteAngle - c->nowAbsoluteAngle) > (PI / 2))
            c->wantedAbsoluteAngle = c->nowAbsoluteAngle + (PI / 2);
        if (radFormat(c->wantedAbsoluteAngle - c->nowAbsoluteAngle) < (-PI / 2))
            c->wantedAbsoluteAngle = c->nowAbsoluteAngle - (PI / 2);
    }
    if (radFormat(gimbalPitchCtrl.wantedAbsoluteAngle) > PITCH_TEST_RAD_MAX)
        gimbalPitchCtrl.wantedAbsoluteAngle = PITCH_TEST_RAD_MAX;
    if (radFormat(gimbalPitchCtrl.wantedAbsoluteAngle) < PITCH_TEST_RAD_MIN)
        gimbalPitchCtrl.wantedAbsoluteAngle = PITCH_TEST_RAD_MIN;
}

#ifdef STM_STUDIO_PITCH_PID
#define LimitMax(input, max)   \
    {                          \
        if (input > max)       \
        {                      \
            input = max;       \
        }                      \
        else if (input < -max) \
        {                      \
            input = -max;      \
        }                      \
    }

fp32 pidPout,pidIout,pidDout;
fp32 PID_calc_STM_STUDIO_PITCH_PID(pid_type_def *pid, fp32 ref, fp32 set,uint8_t i)
{
    if (pid == NULL)
    {
        return 0.0f;
    }

    pid->error[2] = pid->error[1];
    pid->error[1] = pid->error[0];
    pid->set = set;
    pid->fdb = ref;
    pid->error[0] = set - ref;
    if (pid->mode == PID_POSITION)
    {
        pid->Pout = pid->Kp * pid->error[0];
        pid->Iout += pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - pid->error[1]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        LimitMax(pid->Iout, pid->max_iout);
        pid->out = pid->Pout + pid->Iout + pid->Dout;
        LimitMax(pid->out, pid->max_out);
    }
    else if (pid->mode == PID_DELTA)
    {
        pid->Pout = pid->Kp * (pid->error[0] - pid->error[1]);
        pid->Iout = pid->Ki * pid->error[0];
        pid->Dbuf[2] = pid->Dbuf[1];
        pid->Dbuf[1] = pid->Dbuf[0];
        pid->Dbuf[0] = (pid->error[0] - 2.0f * pid->error[1] + pid->error[2]);
        pid->Dout = pid->Kd * pid->Dbuf[0];
        pid->out += pid->Pout + pid->Iout + pid->Dout;
        LimitMax(pid->out, pid->max_out);
    }
    if(i==0){
        pidPout=pid->Pout;
        pidIout=pid->Iout;
        pidDout=pid->Dout;
    }
    return pid->out;
}
#endif

void calc_PID(void)
{
    uint8_t i;
    struct gimbalMotorCtrl_t *c;
    #ifndef SENTRY
    for (i = 0; i < 2; i++)
    {
        c = gimbalCtrl[i];
        gimbal_PID_calc(&(c->aglPid), c->nowAbsoluteAngle, c->wantedAbsoluteAngle); // 关乎旋转方向的PID控制器
        PID_calc(&(c->spdPid), c->radSpeed, (c->aglPid).out);                       // 普通的速度控制环
        c->giveVolt = c->spdPid.out;                                                // 给电压
        if (c->zeroVoltMark == 1)
        {
            c->giveVolt = 0;
        }
    }
    #else
    c = gimbalCtrl[0];
    gimbal_PID_calc(&(c->aglPid), c->nowAbsoluteAngle, c->wantedAbsoluteAngle); // 关乎旋转方向的PID控制器
    #ifndef STM_STUDIO_PITCH_PID
    PID_calc(&(c->spdPid), ((c->radSpeed-(c->aglPid).out)>PID_DEATH_AREA||(c->radSpeed-(c->aglPid).out)<-PID_DEATH_AREA?c->radSpeed:(c->aglPid).out), (c->aglPid).out);                       // 普通的速度控制环
    #else
    PID_calc_STM_STUDIO_PITCH_PID(&(c->spdPid), ((c->radSpeed-(c->aglPid).out)>PID_DEATH_AREA||(c->radSpeed-(c->aglPid).out)<-PID_DEATH_AREA?c->radSpeed:(c->aglPid).out), (c->aglPid).out,i);
    #endif
    c->giveVolt = c->spdPid.out;                                                // 给电压
    if (c->zeroVoltMark == 1)
    {
        c->giveVolt = 0;
    }
    c = gimbalCtrl[1];
    gimbal_PID_calc(&(c->aglPid), c->nowAbsoluteAngle, c->wantedAbsoluteAngle); // 关乎旋转方向的PID控制器
    PID_calc(&(c->spdPid), c->radSpeed, (c->aglPid).out);                         // 普通的速度控制环
    c->giveVolt = c->spdPid.out;                                                // 给电压
    if (c->zeroVoltMark == 1)
    {
        c->giveVolt = 0;
    }
    #endif
}

#ifdef GIMBAL_WITH_IMU

void gimbal_with_IMU_task(void const *pvParameters)
{
    init_gimbal_ctrls();
    osDelay(GIMBAL_TASK_INIT_TIME);
    robotMode = get_robot_present_mode();
    rc_p = get_remote_control_point(); // 获取遥控器数据和NUC数据指针
    nuc_p = get_nuc_control_point();
    remoteControl_p=get_usart_remote_control_p();
    #ifndef SENTRY 
    triggerCurrent_p = get_trigger_current_p(); // 获取shoot_task源文件里计算的M2006的电流。
    #endif
    #ifdef GIMBAL_WITHOUT_IMU
    pitchUpVoltage_p=get_pitch_up_volt_p();
    yawUpVoltage_p=get_yaw_up_volt_p();
    #endif
    while (1)
    {
        refresh_angle_states(); // 从传感器获得当前角度数据 
        get_control_angles();   // 从遥控器获得目标角度     
        limit_angles();
        calc_PID(); // 总是控制角度，双环，但测试时需要
#ifdef HERO
#ifdef ZERO_CURRENT_SAFE_GIMBAL
        CAN_cmd_gimbal(0, 0);
        CAN_cmd_yaw(0);
#else
        CAN_cmd_gimbal(-gimbalPitchCtrl.giveVolt, *triggerCurrent_p); // 使用的反馈值来自IMU,和电机无关，不会正反馈
        CAN_cmd_yaw(gimbalYawCtrl.giveVolt);
#endif
#endif
#ifdef INFANTRY
#ifdef ZERO_CURRENT_SAFE_GIMBAL
        CAN_cmd_gimbal(0, 0);
        CAN_cmd_yaw(0);
#else
        CAN_cmd_gimbal(gimbalPitchCtrl.giveVolt, *triggerCurrent_p);
        CAN_cmd_yaw(gimbalYawCtrl.giveVolt);
#endif
#endif
#ifdef SENTRY
#ifdef ZERO_CURRENT_SAFE_GIMBAL
        gimbalYawCtrl.giveVolt=0;
        gimbalPitchCtrl.giveVolt=0;
#endif
#ifdef GIMBAL_WITHOUT_IMU
        CAN_cmd_yaw_down(gimbalYawCtrl.giveVolt);
        CAN_cmd_GIMBAL_CAN_G(*yawUpVoltage_p,-gimbalPitchCtrl.giveVolt,-*pitchUpVoltage_p);
#else
        CAN_cmd_yaw_down(gimbalYawCtrl.giveVolt);
        CAN_cmd_GIMBAL_CAN_G(0,-gimbalPitchCtrl.giveVolt,0);
#endif
#endif
        osDelay(GIMBAL_TASK_CTRL_TIME);
    }
}
#endif

#ifdef ABSOLUTE_LOCATION
fp32 get_now_yaw_angle(void){
    return gimbalYawCtrl.nowAbsoluteAngle;
}
#endif

#ifdef OLED_GIMBAL_WITH_IMU
void oled_gimbal_with_imu(void){
    OLED_printf(0,0,"yawAngle:%.4f,yawSpeed%.4f,pitchAngle%.4f,pitchSpeed%.4f",gimbalYawCtrl.nowAbsoluteAngle,gimbalYawCtrl.radSpeed,gimbalPitchCtrl.nowAbsoluteAngle,gimbalPitchCtrl.radSpeed);
    // OLED_printf(0,0,"YawAngle:%.4f,YawWantAngle:%.4f,YawSpeed%.4f,YawGiveout:%d",gimbalYawCtrl.nowAbsoluteAngle,gimbalYawCtrl.wantedAbsoluteAngle,gimbalYawCtrl.radSpeed,gimbalYawCtrl.giveVolt);
    // OLED_printf(0,0,"PitchAngle:%.4f,PitchWantAngle:%.4f,PitchSpeed%.4f,PitchGiveout:%d",gimbalPitchCtrl.nowAbsoluteAngle,gimbalPitchCtrl.wantedAbsoluteAngle,gimbalPitchCtrl.radSpeed,gimbalPitchCtrl.giveVolt);
}
#endif
#endif
